#include"route.h"

const MyVec2 connects[] = { MyVec2(0,1),MyVec2(1,0),MyVec2(0,-1),MyVec2(-1,0) };

MyVec2* findPoint(std::list<MyVec2 *>list, int x, int y)
{
	MyVec2 *temp;
	for (auto i : list)
	{
		if (i->x == x&&i->y == y)
		{
			temp = i;
			return temp;
		}
	}
	return nullptr;
}

bool clearPoint(std::list<MyVec2 *>&list, MyVec2* target)
{
	for (auto i : list)
	{
		if (i->x == target->x&&i->y == target->y)
		{
			list.remove(i);
			return true;
		}
	}
	return false;
}

MyVec2* getMin(std::list<MyVec2 *>m_list)
{
	if (!m_list.empty())
	{
		auto res = m_list.front();
		for (auto i : m_list)
		{
			if (res->f > i->f)
				res = i;
		}
		return res;
	}
}

std::stack<MyVec2> Astar(MyVec2 start, MyVec2 end, int (&map)[MAP_W][MAP_H])
{
	std::stack<MyVec2> route;
	std::list<MyVec2 *>open_queue;
	bool isarrvie = false;
	MyVec2 *last = &start;
	MyVec2 *m_start = &start;
	open_queue.push_back(m_start);
	while (!open_queue.empty() && isarrvie == false)
	{
		MyVec2* curr = getMin(open_queue);
		clearPoint(open_queue, curr);
		bool is = clearPoint(open_queue, curr);
		if (map[(int)curr->x][(int)curr->y] != Start && map[(int)curr->x][(int)curr->y] == Space)
		{
			map[(int)curr->x][(int)curr->y] = Visited;
		}
		for (auto dir : connects)
		{
			MyVec2 *temp = new MyVec2(curr->x + dir.x, curr->y + dir.y);
			if (temp->x >= 0 && temp->x < MAP_W&&temp->y < MAP_H&&temp->y >= 0)
			{

				if (map[(int)temp->x][(int)temp->y] == Space)
				{
					temp->parent = curr;
					temp->g = curr->g + 1;
					temp->h = sqrt(pow(abs(temp->x - end.x), 2) + pow(abs(temp->y - end.y), 2));
					temp->f = temp->g + temp->h;
					open_queue.push_back(temp);
				}
				else if (map[(int)temp->x][(int)temp->y] == End)
				{
					isarrvie = true;
					last = curr;
					break;
				}
				else if (findPoint(open_queue, temp->x, temp->y)!=nullptr)
				{
					MyVec2* Temp = findPoint(open_queue, temp->x, temp->y);
					if (Temp->g > curr->g + 1)
					{
						Temp->parent = curr;
						Temp->g = curr->g + 1;
						Temp->f = Temp->g + Temp->h;
					}
				}
			}
		}
	}
	while (last->parent != NULL)
	{
		map[(int)last->x][(int)last->y] = Path;
		route.push(MyVec2(last->x, last->y));
		last = last->parent;
	}
	return route;
}